/*****
 ** @file     : control_config.c/h
 ** @brief    : 控制配置
 *****/

#ifndef CONTROL_CONFIG_H
#define CONTROL_CONFIG_H

#ifdef __cplusplus
extern "C"
{
#endif

#include "kernel_port.h"
#include "stockpile_config.h"

#define MOTOR_DRIVER_PARAM 1  //(电机驱动配置)
#define MOTOR_CONTROL_PARAM 1 //(电机控制配置)

/*******************************************电机驱动配置***********************************************/
#if MOTOR_DRIVER_PARAM
typedef struct
{
    // 电流配置
    uint16_t Rated_Current; // 额定电流(mA)
    uint16_t Cali_Current;  // 校准电流(mA)

    // 运动参数
    // int32_t Step_NUM;             // 单圈步数
    uint16_t Divide_NUM;            // 细分数
    uint16_t Rated_Speed;           // 额定转速(r/s)               = Move_Rated_Speed * Move_Pulse_NUM(Move_Step_NUM * Move_Divide_NUM)
    uint16_t Rated_UpAcc;           // 固件额定加速加速度(r/ss)  = Move_Rated_UpAcc * Move_Pulse_NUM
    uint16_t Rated_DownAcc;         // 固件额定减速加速度(r/ss)    = Move_Rated_DownAcc * Move_Pulse_NUM
    uint16_t Rated_UpCurrentRate;   // 固件额定增流梯度(mA/s)    = Move_Rated_UpCurrentRate * Current_Rated_Current
    uint16_t Rated_DownCurrentRate; // 固件额定减流梯度(mA/s)    = Move_Rated_DownCurrentRate * Current_Rated_Current

} MotorConfig;

extern MotorConfig g_motor_config; // 声明全局配置实例

/********************  硬件配置区  ********************/
#define Current_Rated_Current getCurrentRatedCurrent() // 额定电流(mA)
#define Current_Cali_Current getCurrent_Cali_Current() // 校准电流(mA)

/********************  运动参数配置区  ********************/
#define Move_Step_NUM ((int32_t)(200))                           //(使用的电机单圈步数)(每步磁场旋转90°)
#define Move_Divide_NUM getMoveDivideNum()                       //(每步柔性件控制细分量)
#define Move_Pulse_NUM getMovePulseNum()                         //(电机单圈脉冲数)
#define Move_Rated_Speed getMoveRatedSpeed()                     //(额定转速)(50转每秒)
#define Move_Rated_UpAcc getMoveRatedUpAcc()                     //(固件额定加速加速度)(1000r/ss)
#define Move_Rated_DownAcc getMoveRatedDownAcc()                 //(固件额定减速加速度)(1000r/ss)
#define Move_Rated_UpCurrentRate getMoveRatedUpCurrentRate()     //(固件额定增流梯度)(20倍额定/s)
#define Move_Rated_DownCurrentRate getMoveRatedDownCurrentRate() //(固件额定减流梯度)(20倍额定/s)

/****************************************  校准器配置区  ****************************************/
#define CALI_Encode_Bit ((int32_t)(14))                                     //(编码器位宽)(14位输出精度)
#define CALI_Encode_Res ((int32_t)((0x00000001U) << CALI_Encode_Bit))       //(编码器分辨率)(2^14 = 16384)(16k分辨率)(占用32k校准空间)
#define CALI_Gather_Encode_Res ((int32_t)(CALI_Encode_Res / Move_Step_NUM)) //(校准每采集步编码器分辨率)

/****************************************  控制器频率配置区  ****************************************/
#define CONTROL_FREQ_HZ (20000)                       // 控制频率_hz
#define CONTROL_PERIOD_US (1000000 / CONTROL_FREQ_HZ) // 控制周期_us

    // void Control_Config_Init_Static(void);    //控制静态配置
    // void Control_Config_Init_Dynamic(void);    //控制动态配置

int32_t getCurrentRatedCurrent(void);
int32_t getCurrent_Cali_Current(void);
// int32_t getMoveStepNum(void);
int32_t getMoveDivideNum(void);
int32_t getMovePulseNum(void);
int32_t getMoveRatedSpeed(void);
int32_t getMoveRatedUpAcc(void);
int32_t getMoveRatedDownAcc(void);
int32_t getMoveRatedUpCurrentRate(void);
int32_t getMoveRatedDownCurrentRate(void);

ErrStatus setCurrentRatedCurrent(uint16_t _current);
ErrStatus setCurrent_Cali_Current(uint16_t _current);
ErrStatus setMoveDivideNum(uint16_t _divide);
ErrStatus setMoveRatedSpeed(uint16_t _speed);
ErrStatus setMoveRatedUpAcc(uint16_t _acc);
ErrStatus setMoveRatedDownAcc(uint16_t _acc);
ErrStatus setMoveRatedUpCurrentRate(uint16_t _rate);
ErrStatus setMoveRatedDownCurrentRate(uint16_t _rate);
#endif
/*******************************************电机驱动配置***********************************************/


/*******************************************电机控制配置***********************************************/
#if MOTOR_CONTROL_PARAM
/********************** PID控制(速度控制)  ***********************/
/*********************  PID控制(速度控制)  ***********************/
#define De_PID_KP 5  // 默认KP
#define De_PID_KI 30 // 默认KI
#define De_PID_KD 0  // 默认KD
typedef struct
{
    // 配置
    bool valid_kp, valid_ki, valid_kd; // 参数有效标志
    uint16_t kp, ki, kd;                // 参数
    // 控制参数
    int32_t v_error, v_error_last; // 误差记录
    int32_t op, oi, od;            // 输出
    int32_t i_mut, i_dec;          // 小积分处理
    int32_t out;                   // 输出
} Control_PID_Typedef;
extern Control_PID_Typedef pid;

// 参数配置
ErrStatus Control_PID_SetKP(uint16_t _k); // KP参数配置
ErrStatus Control_PID_SetKI(uint16_t _k); // KI参数配置
ErrStatus Control_PID_SetKD(uint16_t _k); // KD参数配置
void Control_PID_Set_Default(void);  // PID参数恢复
void Control_PID_Init(void);

/******************** DCE控制(位置控制)  **********************/
/******************** DCE控制(位置控制)  **********************/
#define De_DCE_KP 1000                               // 默认KP
#define De_DCE_KI 80                                 // 默认KI
#define De_DCE_KV 300                                // 默认KIV
#define De_DCE_KD 250                                // 默认KD
typedef struct
{
// 配置
    bool valid_kp, valid_ki, valid_kv, valid_kd; // 参数有效标志
    uint16_t kp, ki, kv, kd;                      // 参数
    // 控制参数(基本部分)
    int32_t p_error, v_error; // 误差记录
    int32_t op, oi, od;       // 输出
    int32_t i_mut, i_dec;     // 小积分处理
    int32_t out;              // 输出
} Control_DCE_Typedef;
extern Control_DCE_Typedef dce;

// 参数配置
ErrStatus Control_DCE_SetKP(uint16_t _k); // KP参数配置
ErrStatus Control_DCE_SetKI(uint16_t _k); // KI参数配置
ErrStatus Control_DCE_SetKV(uint16_t _k); // KV参数配置
ErrStatus Control_DCE_SetKD(uint16_t _k); // KD参数配置
void Control_DCE_Set_Default(void);  // DCE参数恢复
void Control_DCE_Init(void);

/********************  Motor_Contro_Debug  *******************/
/********************  Motor_Contro_Debug  *******************/
/**
 * Motor_Control类结构体定义
 **/
typedef struct
{
    // 公共调试
    int32_t mut;   // 调试位置
    int32_t dec;   // 调试位置
    int32_t speed; // 调试速度
} Motor_Control_Debug_Typedef;
extern Motor_Control_Debug_Typedef mc_debug;

/********************  Motor_Control  *********************/
/********************  Motor_Control  *********************/
/**
 * 控制器状态
 **/
typedef enum
{
    Control_State_Stop = 0x00,     // 停止
    Control_State_Finish = 0x01,   // 任务完成
    Control_State_Running = 0x02,  // 任务执行中
    Control_State_Overload = 0x03, // 过载
    Control_State_Stall = 0x04,    // 堵转
    Control_State_Failure = 0x05,  // 任务失败
} Motor_State;

/**
 * 模式
 **/
typedef enum
{
    // 测试
    Motor_Mode_Debug_Location = 0x00, // 调试位置            NULL                                多功能调试信号源
    Motor_Mode_Debug_Speed = 0x01,    // 调试速度            NULL                                多功能调试信号源
                                    // 停止
    Control_Mode_Stop = 0x10, // 停止                NULL                                NULL
                                // DIG(CAN/RS485)
    Motor_Mode_Digital_Location = 0x20, // DIG位置            目标位置                       位置跟踪器(速度,双加速度)
    Motor_Mode_Digital_Speed = 0x21,    // DIG速度            目标速度                       速度跟踪器(双加速度)
    Motor_Mode_Digital_Current = 0x22,  // DIG电流            目标电流                       电流跟踪器(双电梯度)
    Motor_Mode_Digital_Track = 0x23,    // DIG轨迹            多指令                         运动重构器(运动自寻)
                                        // MoreIO(PWM/PUL)
    Motor_Mode_PWM_Location = 0x30,   // PWM位置_舵机        目标位置                        位置跟踪器(速度,双加速度)
    Motor_Mode_PWM_Speed = 0x31,      // PWM速度_调速机      目标速度                        速度跟踪器(双加速度)
    Motor_Mode_PWM_Current = 0x32,    // PWM电流_直流机      目标电流                        电流跟踪器(双电梯度)
    Motor_Mode_PULSE_Location = 0x33, // PULSE位置          目标位置                        位置插补器(无)
} Motor_Mode;

/**
 * Motor_Control类结构体定义
 **/
#define De_Motor_Mode Motor_Mode_PULSE_Location // 默认配置
#define De_Motor_Stall true                     // 默认堵转保护开关
#define De_Motor_Stall_UpRate true              // 默认堵转增流开关
#define De_Motor_Device_ID 0xFE                 // 默认设备ID
typedef struct
{
    // 配置(模式)
    bool valid_mode;       // 有效标志
    Motor_Mode mode_order; // 电机模式_新指令的
    // 配置(堵转)
    bool valid_stall_switch; // 堵转保护开关有效标志
    bool stall_switch;       // 堵转保护开关

    bool valid_stall_UpRate_switch; // 堵ington开关有效标志
    bool stall_UpRate_switch;  // 堵转增流开关

    // 模式
    Motor_Mode mode_run; // 电机模式_运行中的
    // 读取
    int32_t real_lap_location;      // 读取单圈位置
    int32_t real_lap_location_last; // 读取单圈位置
    int32_t real_location;          // 读取位置
    int32_t real_location_last;     // 读取位置
    // 估计
    int32_t est_speed_mut;           // 估计速度倍值(放大n倍)(mut有积分作用,不会有精度损失)
    int32_t est_speed;               // 估计速度
    int32_t est_lead_location;       // 估计位置超前位置
    int32_t est_lead_location_debug; // 估计位置超前位置
    int32_t est_location;            // 估计位置
    int32_t est_error;               // 估计位置误差
    // 硬目标
    int32_t goal_location; // 目标位置(由信号输入)
    int32_t goal_speed;    // 目标速度(由信号输入)
    int16_t goal_current;  // 目标电流(由信号输入)
    bool goal_disable;     // 目标失能(由信号输入)
    bool goal_brake;       // 目标刹车(由信号输入)
    // 软目标
    int32_t soft_location; // 软位置(由 跟踪器/重构器/插补器/硬运算 得到)
    int32_t soft_speed;    // 软速度(由 跟踪器/重构器/插补器/硬运算 得到)
    int16_t soft_current;  // 软电流(由 跟踪器/重构器/插补器/硬运算 得到)
    bool soft_disable;     // 软失能
    bool soft_brake;       // 软刹车
    bool soft_new_curve;   // 新软目标曲线
    // 输出
    int32_t foc_location; // FOC矢量位置
    int32_t foc_current;  // FOC矢量大小
    // 堵转识别
    uint32_t stall_time_us; // 堵转计时器
    bool stall_flag;        // 堵转标志
    // 过载识别
    uint32_t overload_time_us; // 过载计时器
    bool overload_flag;        // 过载标志
    // 状态
    Motor_State state; // 统一的电机状态
    uint8_t device_id;  // 电机ID
} Motor_Control_Typedef;
extern Motor_Control_Typedef motor_control;
extern __IO int32_t step_num;
extern int32_t offset_location;        // 零点偏移位置
extern int32_t reg_location;           // 通信目标位置

// 参数配置
void Motor_Control_SetMotorMode(Motor_Mode _mode); // 控制模式
void Motor_Control_SetStallSwitch(bool _switch);   // 堵转保护开关
void Motor_Control_SetStallUpRateSwitch(bool _upRateSwitch); // 堵转增流开关
void Motor_Control_SetDefault(void);               // 控制模式参数恢复

// 数据写入
void Motor_Control_Write_Goal_Location(int32_t value); // 写入目标位置
void Motor_Control_Write_Goal_Speed(int32_t value);    // 写入目标速度
void Motor_Control_Write_Goal_Current(int16_t value);  // 写入目标电流
void Motor_Control_Write_Goal_Disable(uint16_t value); // 写入目标失能
void Motor_Control_Write_Goal_Brake(uint16_t value);   // 写入目标刹车

void Motor_Device_Id_Set(uint8_t _id);     // 设置设备ID
void Motor_Zero_Set(void);            // 设置零点

#endif
/***************************************电机控制配置************************************************* */

#define CONFIG_PARAM_SAVE_NUM 19 // 配置参数保存数量

extern uint16_t *Read_MOTOR_PARAM_DATA;
// 配置参数保存
ErrStatus Config_Param_save(void);
// 配置参数读取
void Config_Param_Read(void);
#ifdef __cplusplus
}
#endif

#endif
